"""
基础运动控制器
实现通用的运动控制功能
"""

import asyncio
import json
import os
import time
from typing import Dict, Any, Optional, List
from dataclasses import dataclass
from robot_core.servo_controller import ServoController


@dataclass
class ControllerStatus:
    """控制器状态"""

    is_moving: bool = False
    current_action: Optional[str] = None
    servo_positions: Dict[int, int] = None
    battery_level: int = 100
    error: Optional[str] = None


class BaseMotionController:
    """基础运动控制器类"""

    def __init__(self, config: Dict[str, Any]):
        self.config = config
        self.servo_controller = ServoController(config.get("servo", {}))
        self.status = ControllerStatus()
        self.status.servo_positions = {}

        # 加载动作组
        self._load_action_groups()

        # 初始化舵机到初始位置
        self._init_servos()

    def _load_action_groups(self):
        """加载动作组"""
        action_dir = os.path.join("robot_core", "actions")
        if os.path.exists(action_dir):
            for file in os.listdir(action_dir):
                if file.endswith(".json"):
                    with open(os.path.join(action_dir, file), "r") as f:
                        action_data = json.load(f)
                        self._add_action_group(file[:-5], action_data)

    def _add_action_group(self, name: str, data: Dict[str, Any]):
        """添加动作组"""
        # 由子类实现
        pass

    def _init_servos(self):
        """初始化舵机到初始位置"""
        initial_positions = self.config.get("servo", {}).get("initial_positions", {})
        if not initial_positions:
            initial_positions = {
                i: 500 for i in range(1, self.servo_controller.servo_count + 1)
            }
        self.servo_controller.set_positions(initial_positions)
        self.status.servo_positions = initial_positions.copy()

    async def execute_action(
        self, action_name: str, params: Dict[str, Any] = None
    ) -> None:
        """执行动作"""
        try:
            if self.status.is_moving:
                raise RuntimeError("Robot is already moving")

            self.status.is_moving = True
            self.status.current_action = action_name
            self.status.error = None

            action = self._get_action(action_name)
            if not action:
                raise ValueError(f"Action {action_name} not found")

            await self._execute_action_steps(action, params or {})

        except Exception as e:
            self.status.error = str(e)
            raise
        finally:
            self.status.is_moving = False
            self.status.current_action = None

    def _get_action(self, name: str) -> Optional[Dict[str, Any]]:
        """获取动作组"""
        # 由子类实现
        pass

    async def _execute_action_steps(
        self, action: Dict[str, Any], params: Dict[str, Any]
    ) -> None:
        """执行动作步骤"""
        for step in action.get("steps", []):
            if step["type"] == "servo":
                await self._move_servo(
                    step["servo_id"], step["position"], step.get("speed", 1000)
                )
            elif step["type"] == "servos":
                await self._move_servos(step["positions"], step.get("speed", 1000))
            elif step["type"] == "delay":
                await asyncio.sleep(step["duration"] / 1000.0)
            elif step["type"] == "condition":
                if not await self._check_condition(step["condition"], params):
                    break

    async def _move_servo(
        self, servo_id: int, position: int, speed: int = 1000
    ) -> None:
        """移动单个舵机"""
        success = self.servo_controller.set_position(servo_id, position, speed)
        if not success:
            raise RuntimeError(f"Failed to move servo {servo_id}")
        self.status.servo_positions[servo_id] = position
        await asyncio.sleep(0.1)

    async def _move_servos(self, positions: Dict[int, int], speed: int = 1000) -> None:
        """同时移动多个舵机"""
        success = self.servo_controller.set_positions(positions, speed)
        if not success:
            raise RuntimeError("Failed to move servos")
        self.status.servo_positions.update(positions)
        await asyncio.sleep(0.1)

    async def _check_condition(
        self, condition: Dict[str, Any], params: Dict[str, Any]
    ) -> bool:
        """检查条件"""
        condition_type = condition["type"]
        if condition_type == "distance":
            distance = params.get("distance", 0)
            min_dist = condition.get("min", 0)
            max_dist = condition.get("max", float("inf"))
            return min_dist <= distance <= max_dist
        elif condition_type == "time":
            duration = condition.get("duration", 0)
            start_time = params.get("start_time", 0)
            return time.time() - start_time <= duration
        elif condition_type == "servo_position":
            servo_id = condition["servo_id"]
            target_position = condition["position"]
            tolerance = condition.get("tolerance", 10)
            current_position = self.servo_controller.get_position(servo_id)
            return abs(current_position - target_position) <= tolerance
        return True

    async def set_servo_position(
        self, servo_id: int, position: int, speed: int = 1000
    ) -> None:
        """设置单个舵机位置"""
        if self.status.is_moving:
            raise RuntimeError("Cannot move servo while robot is executing an action")
        await self._move_servo(servo_id, position, speed)

    async def get_status(self) -> Dict[str, Any]:
        """获取控制器状态"""
        return {
            "is_moving": self.status.is_moving,
            "current_action": self.status.current_action,
            "servo_positions": self.servo_controller.get_positions(),
            "battery_level": self.status.battery_level,
            "error": self.status.error,
        }

    async def emergency_stop(self) -> None:
        """紧急停止"""
        self.status.is_moving = False
        self.status.current_action = None
        self.servo_controller.emergency_stop()

    def close(self):
        """关闭资源"""
        self.servo_controller.close()
